/*
 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

package org.box2dflash.collision {
	import org.box2dflash.common.*;
	import org.box2dflash.collision.*;
	import org.box2dflash.common.math.*;
	use namespace Internal;

	/*
	This broad phase uses the Sweep and Prune algorithm as described in:
	Collision Detection in Interactive 3D Environments by Gino van den Bergen
	Also, some ideas, such as using integral values for fast compares comes from
	Bullet (http:/www.bulletphysics.com).
	 */


	// Notes:
	// - we use bound arrays instead of linked lists for cache coherence.
	// - we use quantized integral values for fast compares.
	// - we use short indices rather than pointers to save memory.
	// - we use a stabbing count for fast overlap queries (less than order N).
	// - we also use a time stamp on each proxy to speed up the registration of
	//   overlap query results.
	// - where possible, we compare bound indices instead of values to reduce
	//   cache misses (TODO_ERIN).
	// - no broadphase is perfect and neither is this one: it is not great for huge
	//   worlds (use a multi-SAP instead), it is not great for large objects.
	public class BroadPhase {
		//public:
		public function BroadPhase(worldAABB:AABB, callback:PairCallback) {
			//Settings.Assert(worldAABB.IsValid());
			var i:int;
		
			m_pairManager.Initialize(this, callback);
		
			m_worldAABB = worldAABB;
		
			m_proxyCount = 0;
		
			// query results
			for (i = 0;i < Settings._maxProxies; i++) {
				m_queryResults[i] = 0;
			}
		
			// bounds array
			m_bounds = new Array(2);
			for (i = 0;i < 2; i++) {
				m_bounds[i] = new Array(2 * Settings._maxProxies);
				for (var j:int = 0;j < 2 * Settings._maxProxies; j++) {
					m_bounds[i][j] = new Bound();
				}
			}
		
			//Vec2 d = worldAABB.upperBound - worldAABB.lowerBound;
			var dX:Number = worldAABB.upperBound.x - worldAABB.lowerBound.x;;
			var dY:Number = worldAABB.upperBound.y - worldAABB.lowerBound.y;
		
			m_quantizationFactor.x = Settings.USHRT_MAX / dX;
			m_quantizationFactor.y = Settings.USHRT_MAX / dY;
		
			var tProxy:Proxy;
			for (i = 0;i < Settings._maxProxies - 1; ++i) {
				tProxy = new Proxy();
				m_proxyPool[i] = tProxy;
				tProxy.setNext(i + 1);
				tProxy.next;
				tProxy.timeStamp = 0;
				tProxy.overlapCount = _invalid;
				tProxy.userData = null;
			}
			tProxy = new Proxy();
			m_proxyPool[int(Settings._maxProxies - 1)] = tProxy;
			tProxy.setNext(Pair._nullProxy);
			tProxy.next;
			tProxy.timeStamp = 0;
			tProxy.overlapCount = _invalid;
			tProxy.userData = null;
			m_freeProxy = 0;
		
			m_timeStamp = 1;
			m_queryResultCount = 0;
		}

		//~BroadPhase();
	
		// Use this to see if your proxy is in range. If it is not in range,
		// it should be destroyed. Otherwise you may get O(m^2) pairs, where m
		// is the number of proxies that are out of range.
		public function inRange(aabb:AABB):Boolean {
			//Vec2 d = Max(aabb.lowerBound - m_worldAABB.upperBound, m_worldAABB.lowerBound - aabb.upperBound);
			var dX:Number;
			var dY:Number;
			var d2X:Number;
			var d2Y:Number;
		
			dX = aabb.lowerBound.x;
			dY = aabb.lowerBound.y;
			dX -= m_worldAABB.upperBound.x;
			dY -= m_worldAABB.upperBound.y;
		
			d2X = m_worldAABB.lowerBound.x;
			d2Y = m_worldAABB.lowerBound.y;
			d2X -= aabb.upperBound.x;
			d2Y -= aabb.upperBound.y;
		
			dX = Box2dMath.max(dX, d2X);
			dY = Box2dMath.max(dY, d2Y);
		
			return Box2dMath.max(dX, dY) < 0.0;
		}

		// Get a single proxy. Returns NULL if the id is invalid.
		public function proxy(proxyId:int):Proxy {
			var proxy:Proxy = m_proxyPool[proxyId];
			if (proxyId == Pair._nullProxy || proxy.isValid == false) {
				return null;
			}
		
			return proxy;
		}

		// Create and destroy proxies. These call Flush first.
		public function createProxy(aabb:AABB, userData:*):uint {
			var index:uint;
			var proxy:Proxy;
		
			//Settings.Assert(m_proxyCount < _maxProxies);
			//Settings.Assert(m_freeProxy != Pair._nullProxy);
			var proxyId:uint = m_freeProxy;
			proxy = m_proxyPool[ proxyId ];
			m_freeProxy = proxy.next;
		
			proxy.overlapCount = 0;
			proxy.userData = userData;
		
			var boundCount:uint = 2 * m_proxyCount;
		
			var lowerValues:Array = new Array();
			var upperValues:Array = new Array();
			computeBounds(lowerValues, upperValues, aabb);
		
			for (var axis:int = 0;axis < 2; ++axis) {
				var bounds:Array = m_bounds[axis];
				var lowerIndex:uint;
				var upperIndex:uint;
				var lowerIndexOut:Array = [lowerIndex];
				var upperIndexOut:Array = [upperIndex];
				query(lowerIndexOut, upperIndexOut, lowerValues[axis], upperValues[axis], bounds, boundCount, axis);
				lowerIndex = lowerIndexOut[0];
				upperIndex = upperIndexOut[0];
			
				// Replace memmove calls
				//memmove(bounds + upperIndex + 2, bounds + upperIndex, (edgeCount - upperIndex) * sizeof(Bound));
				var tArr:Array = new Array();
				var j:int;
				var tEnd:int = boundCount - upperIndex;
				var tBound1:Bound;
				var tBound2:Bound;
				var tBoundAS3:Bound;
				// make temp array
				for (j = 0;j < tEnd; j++) {
					tArr[j] = new Bound();
					tBound1 = tArr[j];
					tBound2 = bounds[int(upperIndex + j)];
					tBound1.value = tBound2.value;
					tBound1.proxyId = tBound2.proxyId;
					tBound1.stabbingCount = tBound2.stabbingCount;
				}
				// move temp array back in to bounds
				tEnd = tArr.length;
				var tIndex:int = upperIndex + 2;
				for (j = 0;j < tEnd; j++) {
					//bounds[tIndex+j] = tArr[j];
					tBound2 = tArr[j];
					tBound1 = bounds[int(tIndex + j)];
					tBound1.value = tBound2.value;
					tBound1.proxyId = tBound2.proxyId;
					tBound1.stabbingCount = tBound2.stabbingCount;
				}
				//memmove(bounds + lowerIndex + 1, bounds + lowerIndex, (upperIndex - lowerIndex) * sizeof(Bound));
				// make temp array
				tArr = new Array();
				tEnd = upperIndex - lowerIndex;
				for (j = 0;j < tEnd; j++) {
					tArr[j] = new Bound();
					tBound1 = tArr[j];
					tBound2 = bounds[int(lowerIndex + j)];
					tBound1.value = tBound2.value;
					tBound1.proxyId = tBound2.proxyId;
					tBound1.stabbingCount = tBound2.stabbingCount;
				}
				// move temp array back in to bounds
				tEnd = tArr.length;
				tIndex = lowerIndex + 1;
				for (j = 0;j < tEnd; j++) {
					//bounds[tIndex+j] = tArr[j];
					tBound2 = tArr[j];
					tBound1 = bounds[int(tIndex + j)];
					tBound1.value = tBound2.value;
					tBound1.proxyId = tBound2.proxyId;
					tBound1.stabbingCount = tBound2.stabbingCount;
				}
			
				// The upper index has increased because of the lower bound insertion.
				++upperIndex;
			
				// Copy in the new bounds.
				tBound1 = bounds[lowerIndex];
				tBound2 = bounds[upperIndex];
				tBound1.value = lowerValues[axis];
				tBound1.proxyId = proxyId;
				tBound2.value = upperValues[axis];
				tBound2.proxyId = proxyId;
			
				tBoundAS3 = bounds[int(lowerIndex - 1)];
				tBound1.stabbingCount = lowerIndex == 0 ? 0 : tBoundAS3.stabbingCount;
				tBoundAS3 = bounds[int(upperIndex - 1)];
				tBound2.stabbingCount = tBoundAS3.stabbingCount;
			
				// Adjust the stabbing count between the new bounds.
				for (index = lowerIndex;index < upperIndex; ++index) {
					tBoundAS3 = bounds[index];
					tBoundAS3.stabbingCount++;
				}
			
				// Adjust the all the affected bound indices.
				for (index = lowerIndex;index < boundCount + 2; ++index) {
					tBound1 = bounds[index];
					var proxy2:Proxy = m_proxyPool[ tBound1.proxyId ];
					if (tBound1.isLower) {
						proxy2.lowerBounds[axis] = index;
					}
				else {
						proxy2.upperBounds[axis] = index;
					}
				}
			}
		
			++m_proxyCount;
		
			//Settings.Assert(m_queryResultCount < Settings._maxProxies);
			for (var i:int = 0;i < m_queryResultCount; ++i) {
				//Settings.Assert(m_queryResults[i] < _maxProxies);
				//Settings.Assert(m_proxyPool[m_queryResults[i]].IsValid());
				m_pairManager.AddBufferedPair(proxyId, m_queryResults[i]);
			}
		
			m_pairManager.Commit();
		
			// Prepare for next query.
			m_queryResultCount = 0;
			incrementTimeStamp();
		
			return proxyId;
		}

		public function destroyProxy(proxyId:uint):void {
			var tBound1:Bound;
			var tBound2:Bound;
		
			//Settings.Assert(0 < m_proxyCount && m_proxyCount <= _maxProxies);
			var proxy:Proxy = m_proxyPool[ proxyId ];
			//Settings.Assert(proxy.IsValid());
			var boundCount:int = 2 * m_proxyCount;
		
			for (var axis:int = 0;axis < 2; ++axis) {
				var bounds:Array = m_bounds[axis];
			
				var lowerIndex:uint = proxy.lowerBounds[axis];
				var upperIndex:uint = proxy.upperBounds[axis];
				tBound1 = bounds[lowerIndex];
				var lowerValue:uint = tBound1.value;
				tBound2 = bounds[upperIndex];
				var upperValue:uint = tBound2.value;
			
				// replace memmove calls
				//memmove(bounds + lowerIndex, bounds + lowerIndex + 1, (upperIndex - lowerIndex - 1) * sizeof(Bound));
				var tArr:Array = new Array();
				var j:int;
				var tEnd:int = upperIndex - lowerIndex - 1;
				// make temp array
				for (j = 0;j < tEnd; j++) {
					tArr[j] = new Bound();
					tBound1 = tArr[j];
					tBound2 = bounds[int(lowerIndex + 1 + j)];
					tBound1.value = tBound2.value;
					tBound1.proxyId = tBound2.proxyId;
					tBound1.stabbingCount = tBound2.stabbingCount;
				}
				// move temp array back in to bounds
				tEnd = tArr.length;
				var tIndex:int = lowerIndex;
				for (j = 0;j < tEnd; j++) {
					//bounds[tIndex+j] = tArr[j];
					tBound2 = tArr[j];
					tBound1 = bounds[int(tIndex + j)];
					tBound1.value = tBound2.value;
					tBound1.proxyId = tBound2.proxyId;
					tBound1.stabbingCount = tBound2.stabbingCount;
				}
				//memmove(bounds + upperIndex-1, bounds + upperIndex + 1, (edgeCount - upperIndex - 1) * sizeof(Bound));
				// make temp array
				tArr = new Array();
				tEnd = boundCount - upperIndex - 1;
				for (j = 0;j < tEnd; j++) {
					tArr[j] = new Bound();
					tBound1 = tArr[j];
					tBound2 = bounds[int(upperIndex + 1 + j)];
					tBound1.value = tBound2.value;
					tBound1.proxyId = tBound2.proxyId;
					tBound1.stabbingCount = tBound2.stabbingCount;
				}
				// move temp array back in to bounds
				tEnd = tArr.length;
				tIndex = upperIndex - 1;
				for (j = 0;j < tEnd; j++) {
					//bounds[tIndex+j] = tArr[j];
					tBound2 = tArr[j];
					tBound1 = bounds[int(tIndex + j)];
					tBound1.value = tBound2.value;
					tBound1.proxyId = tBound2.proxyId;
					tBound1.stabbingCount = tBound2.stabbingCount;
				}
			
				// Fix bound indices.
				tEnd = boundCount - 2;
				for (var index:uint = lowerIndex;index < tEnd; ++index) {
					tBound1 = bounds[index];
					var proxy2:Proxy = m_proxyPool[ tBound1.proxyId ];
					if (tBound1.isLower) {
						proxy2.lowerBounds[axis] = index;
					}
				else {
						proxy2.upperBounds[axis] = index;
					}
				}
			
				// Fix stabbing count.
				tEnd = upperIndex - 1;
				for (var index2:int = lowerIndex;index2 < tEnd; ++index2) {
					tBound1 = bounds[index2];
					tBound1.stabbingCount--;
				}
			
				// Query for pairs to be removed. lowerIndex and upperIndex are not needed.
				// make lowerIndex and upper output using an array and do this for others if compiler doesn't pick them up
				query([0], [0], lowerValue, upperValue, bounds, boundCount - 2, axis);
			}
		
			//Settings.Assert(m_queryResultCount < Settings._maxProxies);
			for (var i:int = 0;i < m_queryResultCount; ++i) {
				//Settings.Assert(m_proxyPool[m_queryResults[i]].IsValid());
				m_pairManager.RemoveBufferedPair(proxyId, m_queryResults[i]);
			}
		
			m_pairManager.Commit();
		
			// Prepare for next query.
			m_queryResultCount = 0;
			incrementTimeStamp();
		
			// Return the proxy to the pool.
			proxy.userData = null;
			proxy.overlapCount = _invalid;
			proxy.lowerBounds[0] = _invalid;
			proxy.lowerBounds[1] = _invalid;
			proxy.upperBounds[0] = _invalid;
			proxy.upperBounds[1] = _invalid;
		
			proxy.setNext(m_freeProxy);
			proxy.next;
			m_freeProxy = proxyId;
			--m_proxyCount;
		}

		// Call MoveProxy as many times as you like, then when you are done
		// call Commit to finalized the proxy pairs (for your time step).
		public function moveProxy(proxyId:uint, aabb:AABB):void {
			var as3arr:Array;
			var as3int:int;
		
			var axis:uint;
			var index:uint;
			var bound:Bound;
			var prevBound:Bound;
			var nextBound:Bound;
			var nextProxyId:uint;
			var nextProxy:Proxy;
		
			if (proxyId == Pair._nullProxy || Settings._maxProxies <= proxyId) {
				//Settings.Assert(false);
				return;
			}
		
			if (aabb.isValid == false) {
				//Settings.Assert(false);
				return;
			}
		
			var boundCount:uint = 2 * m_proxyCount;
		
			var proxy:Proxy = m_proxyPool[ proxyId ];
			// Get new bound values
			var newValues:BoundValues = new BoundValues();
			computeBounds(newValues.lowerValues, newValues.upperValues, aabb);
		
			// Get old bound values
			var oldValues:BoundValues = new BoundValues();
			for (axis = 0;axis < 2; ++axis) {
				bound = m_bounds[axis][proxy.lowerBounds[axis]];
				oldValues.lowerValues[axis] = bound.value;
				bound = m_bounds[axis][proxy.upperBounds[axis]];
				oldValues.upperValues[axis] = bound.value;
			}
		
			for (axis = 0;axis < 2; ++axis) {
				var bounds:Array = m_bounds[axis];
			
				var lowerIndex:uint = proxy.lowerBounds[axis];
				var upperIndex:uint = proxy.upperBounds[axis];
			
				var lowerValue:uint = newValues.lowerValues[axis];
				var upperValue:uint = newValues.upperValues[axis];
			
				bound = bounds[lowerIndex];
				var deltaLower:int = lowerValue - bound.value;
				bound.value = lowerValue;
			
				bound = bounds[upperIndex];
				var deltaUpper:int = upperValue - bound.value;
				bound.value = upperValue;
			
				//
				// Expanding adds overlaps
				//
			
				// Should we move the lower bound down?
				if (deltaLower < 0) {
					index = lowerIndex;
					while (index > 0 && lowerValue < (bounds[int(index - 1)] as Bound).value) {
						bound = bounds[index];
						prevBound = bounds[int(index - 1)];
					
						var prevProxyId:uint = prevBound.proxyId;
						var prevProxy:Proxy = m_proxyPool[ prevBound.proxyId ];
					
						prevBound.stabbingCount++;
					
						if (prevBound.isUpper == true) {
							if (testOverlap(newValues, prevProxy)) {
								m_pairManager.AddBufferedPair(proxyId, prevProxyId);
							}
						
							//prevProxy.upperBounds[axis]++;
							as3arr = prevProxy.upperBounds;
							as3int = as3arr[axis];
							as3int++;
							as3arr[axis] = as3int;
						
							bound.stabbingCount++;
						}
					else {
							//prevProxy.lowerBounds[axis]++;
							as3arr = prevProxy.lowerBounds;
							as3int = as3arr[axis];
							as3int++;
							as3arr[axis] = as3int;
						
							bound.stabbingCount--;
						}
					
						//proxy.lowerBounds[axis]--;
						as3arr = proxy.lowerBounds;
						as3int = as3arr[axis];
						as3int--;
						as3arr[axis] = as3int;
					
						// swap
						//var temp:Bound = bound;
						//bound = prevEdge;
						//prevEdge = temp;
						bound.swap(prevBound);
						//Box2dMath.Swap(bound, prevEdge);
						--index;
					}
				}
			
				// Should we move the upper bound up?
				if (deltaUpper > 0) {
					index = upperIndex;
					while (index < boundCount - 1 && (bounds[int(index + 1)] as Bound).value <= upperValue) {
						bound = bounds[ index ];
						nextBound = bounds[ int(index + 1) ];
						nextProxyId = nextBound.proxyId;
						nextProxy = m_proxyPool[ nextProxyId ];
					
						nextBound.stabbingCount++;
					
						if (nextBound.isLower == true) {
							if (testOverlap(newValues, nextProxy)) {
								m_pairManager.AddBufferedPair(proxyId, nextProxyId);
							}
						
							//nextProxy.lowerBounds[axis]--;
							as3arr = nextProxy.lowerBounds;
							as3int = as3arr[axis];
							as3int--;
							as3arr[axis] = as3int;
						
							bound.stabbingCount++;
						}
					else {
							//nextProxy.upperBounds[axis]--;
							as3arr = nextProxy.upperBounds;
							as3int = as3arr[axis];
							as3int--;
							as3arr[axis] = as3int;
						
							bound.stabbingCount--;
						}
					
						//proxy.upperBounds[axis]++;
						as3arr = proxy.upperBounds;
						as3int = as3arr[axis];
						as3int++;
						as3arr[axis] = as3int;
					
						// swap
						//var temp:Bound = bound;
						//bound = nextEdge;
						//nextEdge = temp;
						bound.swap(nextBound);
						//Box2dMath.Swap(bound, nextEdge);
						index++;
					}
				}
			
				//
				// Shrinking removes overlaps
				//
			
				// Should we move the lower bound up?
				if (deltaLower > 0) {
					index = lowerIndex;
					while (index < boundCount - 1 && (bounds[int(index + 1)] as Bound).value <= lowerValue) {
						bound = bounds[ index ];
						nextBound = bounds[ int(index + 1) ];
					
						nextProxyId = nextBound.proxyId;
						nextProxy = m_proxyPool[ nextProxyId ];
					
						nextBound.stabbingCount--;
					
						if (nextBound.isUpper) {
							if (testOverlap(oldValues, nextProxy)) {
								m_pairManager.RemoveBufferedPair(proxyId, nextProxyId);
							}
						
							//nextProxy.upperBounds[axis]--;
							as3arr = nextProxy.upperBounds;
							as3int = as3arr[axis];
							as3int--;
							as3arr[axis] = as3int;
						
							bound.stabbingCount--;
						}
					else {
							//nextProxy.lowerBounds[axis]--;
							as3arr = nextProxy.lowerBounds;
							as3int = as3arr[axis];
							as3int--;
							as3arr[axis] = as3int;
						
							bound.stabbingCount++;
						}
					
						//proxy.lowerBounds[axis]++;
						as3arr = proxy.lowerBounds;
						as3int = as3arr[axis];
						as3int++;
						as3arr[axis] = as3int;
					
						// swap
						//var temp:Bound = bound;
						//bound = nextEdge;
						//nextEdge = temp;
						bound.swap(nextBound);
						//Box2dMath.Swap(bound, nextEdge);
						index++;
					}
				}
			
				// Should we move the upper bound down?
				if (deltaUpper < 0) {
					index = upperIndex;
					while (index > 0 && upperValue < (bounds[int(index - 1)] as Bound).value) {
						bound = bounds[index];
						prevBound = bounds[int(index - 1)];
					
						prevProxyId = prevBound.proxyId;
						prevProxy = m_proxyPool[ prevProxyId ];
					
						prevBound.stabbingCount--;
					
						if (prevBound.isLower == true) {
							if (testOverlap(oldValues, prevProxy)) {
								m_pairManager.RemoveBufferedPair(proxyId, prevProxyId);
							}
						
							//prevProxy.lowerBounds[axis]++;
							as3arr = prevProxy.lowerBounds;
							as3int = as3arr[axis];
							as3int++;
							as3arr[axis] = as3int;
						
							bound.stabbingCount--;
						}
					else {
							//prevProxy.upperBounds[axis]++;
							as3arr = prevProxy.upperBounds;
							as3int = as3arr[axis];
							as3int++;
							as3arr[axis] = as3int;
						
							bound.stabbingCount++;
						}
					
						//proxy.upperBounds[axis]--;
						as3arr = proxy.upperBounds;
						as3int = as3arr[axis];
						as3int--;
						as3arr[axis] = as3int;
					
						// swap
						//var temp:Bound = bound;
						//bound = prevEdge;
						//prevEdge = temp;
						bound.swap(prevBound);
						//Box2dMath.Swap(bound, prevEdge);
						index--;
					}
				}
			}
		}

		public function commit():void {
			m_pairManager.Commit();
		}

		// Query an AABB for overlapping proxies, returns the user data and
		// the count, up to the supplied maximum count.
		public function queryAABB(aabb:AABB, userData:*, maxCount:int):int {
			var lowerValues:Array = new Array();
			var upperValues:Array = new Array();
			computeBounds(lowerValues, upperValues, aabb);
		
			var lowerIndex:uint;
			var upperIndex:uint;
			var lowerIndexOut:Array = [lowerIndex];
			var upperIndexOut:Array = [upperIndex];
			query(lowerIndexOut, upperIndexOut, lowerValues[0], upperValues[0], m_bounds[0], 2 * m_proxyCount, 0);
			query(lowerIndexOut, upperIndexOut, lowerValues[1], upperValues[1], m_bounds[1], 2 * m_proxyCount, 1);
		
			//Settings.Assert(m_queryResultCount < Settings._maxProxies);
			var count:int = 0;
			for (var i:int = 0;i < m_queryResultCount && count < maxCount; ++i, ++count) {
				//Settings.Assert(m_queryResults[i] < Settings._maxProxies);
				var proxy:Proxy = m_proxyPool[ m_queryResults[i] ];
				//Settings.Assert(proxy.isValid);
				userData[i] = proxy.userData;
			}
		
			// Prepare for next query.
			m_queryResultCount = 0;
			incrementTimeStamp();
		
			return count;
		}

		public function validate():void {
			
			//NEVER USED
			
//			var pair:Pair;
//			var proxy1:Proxy;
//			var proxy2:Proxy;
//			var overlap:Boolean;
		
			for (var axis:int = 0;axis < 2; ++axis) {
				var bounds:Bound = m_bounds[axis];
			
				var boundCount:uint = 2 * m_proxyCount;
				var stabbingCount:uint = 0;
			
				for (var i:uint = 0;i < boundCount; ++i) {
					var bound:Bound = bounds[i];
					//Settings.Assert(i == 0 || bounds[i-1].value <= bound->value);
					//Settings.Assert(bound->proxyId != _nullProxy);
					//Settings.Assert(m_proxyPool[bound->proxyId].isValid);
					if (bound.isLower == true) {
						//Settings.Assert(m_proxyPool[bound.proxyId].lowerBounds[axis] == i);
						stabbingCount++;
					}
				else {
						//Settings.Assert(m_proxyPool[bound.proxyId].upperBounds[axis] == i);
						stabbingCount--;
					}
				
				//Settings.Assert(bound.stabbingCount == stabbingCount);
				}
			}
		}

		//private:
		private function computeBounds(lowerValues:Array, upperValues:Array, aabb:AABB):void {
			//Settings.Assert(aabb.upperBound.x > aabb.lowerBound.x);
			//Settings.Assert(aabb.upperBound.y > aabb.lowerBound.y);
		
			//var minVertex:Vec2 = Box2dMath.ClampV(aabb.minVertex, m_worldAABB.minVertex, m_worldAABB.maxVertex);
			var minVertexX:Number = aabb.lowerBound.x;
			var minVertexY:Number = aabb.lowerBound.y;
			minVertexX = Box2dMath.min(minVertexX, m_worldAABB.upperBound.x);
			minVertexY = Box2dMath.min(minVertexY, m_worldAABB.upperBound.y);
			minVertexX = Box2dMath.max(minVertexX, m_worldAABB.lowerBound.x);
			minVertexY = Box2dMath.max(minVertexY, m_worldAABB.lowerBound.y);
		
			//var maxVertex:Vec2 = Box2dMath.ClampV(aabb.maxVertex, m_worldAABB.minVertex, m_worldAABB.maxVertex);
			var maxVertexX:Number = aabb.upperBound.x;
			var maxVertexY:Number = aabb.upperBound.y;
			maxVertexX = Box2dMath.min(maxVertexX, m_worldAABB.upperBound.x);
			maxVertexY = Box2dMath.min(maxVertexY, m_worldAABB.upperBound.y);
			maxVertexX = Box2dMath.max(maxVertexX, m_worldAABB.lowerBound.x);
			maxVertexY = Box2dMath.max(maxVertexY, m_worldAABB.lowerBound.y);
		
			// Bump lower bounds downs and upper bounds up. This ensures correct sorting of
			// lower/upper bounds that would have equal values.
			// TODO_ERIN implement fast float to uint16 conversion.
			lowerValues[0] = uint(m_quantizationFactor.x * (minVertexX - m_worldAABB.lowerBound.x)) & (Settings.USHRT_MAX - 1);
			upperValues[0] = (uint(m_quantizationFactor.x * (maxVertexX - m_worldAABB.lowerBound.x)) & 0x0000ffff) | 1;
		
			lowerValues[1] = uint(m_quantizationFactor.y * (minVertexY - m_worldAABB.lowerBound.y)) & (Settings.USHRT_MAX - 1);
			upperValues[1] = (uint(m_quantizationFactor.y * (maxVertexY - m_worldAABB.lowerBound.y)) & 0x0000ffff) | 1;
		}

		// This one is only used for validation.
		
		//NEVER USED
		
//		private function TestOverlapValidate(p1:Proxy, p2:Proxy):Boolean {
//		
//			for (var axis:int = 0;axis < 2; ++axis) {
//				var bounds:Array = m_bounds[axis];
//			
//				//Settings.Assert(p1.lowerBounds[axis] < 2 * m_proxyCount);
//				//Settings.Assert(p1.upperBounds[axis] < 2 * m_proxyCount);
//				//Settings.Assert(p2.lowerBounds[axis] < 2 * m_proxyCount);
//				//Settings.Assert(p2.upperBounds[axis] < 2 * m_proxyCount);
//				var bound1:Bound = bounds[p1.lowerBounds[axis]];
//				var bound2:Bound = bounds[p2.upperBounds[axis]];
//				if (bound1.value > bound2.value)
//				return false;
//			
//				bound1 = bounds[p1.upperBounds[axis]];
//				bound2 = bounds[p2.lowerBounds[axis]];
//				if (bound1.value < bound2.value)
//				return false;
//			}
//		
//			return true;
//		}

		public function testOverlap(b:BoundValues, p:Proxy):Boolean {
			for (var axis:int = 0;axis < 2; ++axis) {
				var bounds:Array = m_bounds[axis];
			
				//Settings.Assert(p.lowerBounds[axis] < 2 * m_proxyCount);
				//Settings.Assert(p.upperBounds[axis] < 2 * m_proxyCount);
				var bound:Bound = bounds[p.upperBounds[axis]];
				if (b.lowerValues[axis] > bound.value)
				return false;
			
				bound = bounds[p.lowerBounds[axis]];
				if (b.upperValues[axis] < bound.value)
				return false;
			}
		
			return true;
		}

		private function query(lowerQueryOut:Array, upperQueryOut:Array, lowerValue:uint, upperValue:uint, bounds:Array, boundCount:uint, axis:int):void {
		
			var lowerQuery:uint = binarySearch(bounds, boundCount, lowerValue);
			var upperQuery:uint = binarySearch(bounds, boundCount, upperValue);
			var bound:Bound;
		
			// Easy case: lowerQuery <= lowerIndex(i) < upperQuery
			// Solution: search query range for min bounds.
			for (var j:uint = lowerQuery;j < upperQuery; ++j) {
				bound = bounds[j];
				if (bound.isLower) {
					incrementOverlapCount(bound.proxyId);
				}
			}
		
			// Hard case: lowerIndex(i) < lowerQuery < upperIndex(i)
			// Solution: use the stabbing count to search down the bound array.
			if (lowerQuery > 0) {
				var i:int = lowerQuery - 1;
				bound = bounds[i];
				var s:int = bound.stabbingCount;
			
				// Find the s overlaps.
				while (s) {
					//Settings.Assert(i >= 0);
					bound = bounds[i];
					if (bound.isLower) {
						var proxy:Proxy = m_proxyPool[ bound.proxyId ];
						if (lowerQuery <= proxy.upperBounds[axis]) {
							incrementOverlapCount(bound.proxyId);
							--s;
						}
					}
					--i;
				}
			}
		
			lowerQueryOut[0] = lowerQuery;
			upperQueryOut[0] = upperQuery;
		}

		private function incrementOverlapCount(proxyId:uint):void {
			var proxy:Proxy = m_proxyPool[ proxyId ];
			if (proxy.timeStamp < m_timeStamp) {
				proxy.timeStamp = m_timeStamp;
				proxy.overlapCount = 1;
			}
		else {
				proxy.overlapCount = 2;
				//Settings.Assert(m_queryResultCount < Settings._maxProxies);
				m_queryResults[m_queryResultCount] = proxyId;
				++m_queryResultCount;
			}
		}

		private function incrementTimeStamp():void {
			if (m_timeStamp == Settings.USHRT_MAX) {
				for (var i:uint = 0;i < Settings._maxProxies; ++i) {
					(m_proxyPool[i] as Proxy).timeStamp = 0;
				}
				m_timeStamp = 1;
			}
		else {
				++m_timeStamp;
			}
		}

		//public:
		public var m_pairManager:PairManager = new PairManager();
		public var m_proxyPool:Array = new Array(Settings._maxPairs);
		public var m_freeProxy:uint = new uint();
		public var m_bounds:Array = new Array(2 * Settings._maxProxies);
		public var m_queryResults:Array = new Array(Settings._maxProxies);
		public var m_queryResultCount:int;
		public var m_worldAABB:AABB;
		public var m_quantizationFactor:Vec2 = new Vec2();
		public var m_proxyCount:int;
		public var m_timeStamp:uint;
		static public var s_validate:Boolean = false;
		static public const _invalid:uint = Settings.USHRT_MAX;
		static public const _nullEdge:uint = Settings.USHRT_MAX;

		static public function binarySearch(bounds:Array, count:int, value:uint):uint {
			var low:int = 0;
			var high:int = count - 1;
			while (low <= high) {
				var mid:int = ((low + high) / 2);
				var bound:Bound = bounds[mid];
				if (bound.value > value) {
					high = mid - 1;
				}
			else if (bound.value < value) {
					low = mid + 1;
				}
			else {
					return uint(mid);
				}
			}
		
			return uint(low);
		}
	}
}
